 #include "TargetTracker2012.h"

namespace Team675
{

TargetTracker2012::TargetTracker2012() {
	m_camera = &AxisCamera::GetInstance();
	printf("Setting camera parameters\n");
	//m_camera->WriteResolution(AxisCamera::kResolution_320x240);
	printf("camera initalized");
}

LaunchInfo TargetTracker2012::Update(Target::TargetPosition _targetPos, bool _swish) {
	LaunchInfo launchInfo;
	if(m_camera->IsFreshImage()) {
		printf("camera is fresh");
		m_binImage = ProcessImage();
		vector<ParticleAnalysisReport> *particles = m_binImage->GetOrderedParticleAnalysisReports();
		GetTargets(*particles);
		Position robotPos = GetRobotPosition();
		if(robotPos.x != 0 || robotPos.z != 0) {
			launchInfo = GetLaunchVelocity(robotPos, _targetPos, _swish);
		}
		launchInfo.Print();
		delete particles;
		delete m_binImage;
	}

	return launchInfo;
}

LaunchInfo TargetTracker2012::Update(Target::TargetPosition _targetLevel) {
	return Update(_targetLevel, true);
}

LaunchInfo TargetTracker2012::Update() {
	return Update(Target::TARGET_TOP);
}

BinaryImage *TargetTracker2012::ProcessImage() {
	ColorImage *img = m_camera->GetImage();
	imgWidth = img->GetWidth();
	imgHeight = img->GetHeight();
	BinaryImage *output = img->ThresholdRGB(TARGET_COLOR_THRESHOLD);
	Image *imaqImage = output->GetImaqImage();
	
	//remove small particles
	int pKernel[9] = {1,1,1,1,1,1,1,1,1};
	StructuringElement structElem;
	structElem.matrixCols = 3;
	structElem.matrixRows = 3;
	structElem.hexa = FALSE;
	structElem.kernel = pKernel;
	imaqSizeFilter(imaqImage,imaqImage,TRUE, 1, IMAQ_KEEP_LARGE, &structElem);
	
	//convex hull
	imaqConvexHull(imaqImage,imaqImage, TRUE);
	
	delete img;
	return output;
}

float TargetTracker2012::GetAzimuthFromWidth(Target::TargetPosition _targPos) //PHI IS ALWAYS POSITIVE!!!!!!!!!!!!
{
	Position robotPos;
	Target &target = m_targets[_targPos];
	float theta = target.m_targetAngleWidth;
	float phi; // real value
	float phi1;
	float phi2;
	float G = TARGET_WIDTH / target.m_distance;
	float H = G * G;
	float U = tan(theta / 2); 
	float A = H * U * U * U * U;
	float B = -2 * (U * U) * (H + 2); 
	float C = H - (4 * U * U);

	float result1 = MathExtension::SolveSecondDegree(A,B,C,true);
	float result2 = MathExtension::SolveSecondDegree(A,B,C,false);
	
	phi1 = atan(sqrt(result1));
	phi2 = atan(sqrt(result2));
	
	//result one 
	float test1 =  cos(phi1) * (tan(phi1 + theta/2) - tan(phi1 - theta / 2));
	float test2 =  cos(phi2) * (tan(phi2 + theta/2) - tan(phi2 - theta / 2));
	
	if (fabs(test1 - G) > fabs(test2 - G))
	{
		phi = phi2;
	}
	else 
	{
		phi = phi1; 
	}

	return phi;
}



/*float TargetTracker2012::GetHorizAngle(int pixelX, int pixelY) {
	pixelX -= imgWidth/2;
	pixelY = imgHeight/2 - pixelY;
	float pixelR = sqrt(pixelX*pixelX+pixelY*pixelY);
	float scaleFactor = 1 - 0.000626*pixelR + 0.00000645*pixelR*pixelR;
	float xPixel = scaleFactor*pixelX;
	float radPerPixel =.0025;
	float angle = atan(xPixel * radPerPixel);
	angle += RADIANS_PER_DEGREE * HORIZONTAL_CAMERA_ANGLE_OFFSET;
	return angle;
}

float TargetTracker2012::GetVertAngle(int pixelX, int pixelY) {
	pixelX -= imgWidth/2;
	pixelY = imgHeight/2 - pixelY;
	float pixelR = sqrt(pixelX*pixelX+pixelY*pixelY);
	float scaleFactor = 1 - 0.000626*pixelR + 0.00000645*pixelR*pixelR;
	float yPixel = scaleFactor * pixelY;
	float radPerPixel = .0025;
	float angle = atan(yPixel * radPerPixel);
	angle += RADIANS_PER_DEGREE * VERTICAL_CAMERA_ANGLE_OFFSET;
	return angle;
}*/

float TargetTracker2012::GetHorizAngle(int pixelX, int pixelY) {
	float angle = atan((2.0 * pixelX / imgWidth - 1) * tan(RADIANS_PER_DEGREE * HORIZONTAL_FOV / 2));
	angle += RADIANS_PER_DEGREE * HORIZONTAL_CAMERA_ANGLE_OFFSET;
	return angle;
}

float TargetTracker2012::GetVertAngle(int pixelX, int pixelY) {
	float angle = atan((1 - 2.0 * pixelY / imgHeight) * tan(RADIANS_PER_DEGREE * VERTICAL_FOV / 2));
	angle += RADIANS_PER_DEGREE * VERTICAL_CAMERA_ANGLE_OFFSET;
	return angle;
}

float TargetTracker2012::GetDist(float _height, float _angle) {
	return (_height - CAMERA_HEIGHT)/ tan(_angle);
}

void TargetTracker2012::GetDistAndHeight(ParticleAnalysisReport &target, float &dist, float &height) {
	Rect rect = target.boundingRect;
	double tanBottomAngle = tan(GetVertAngle(target.center_mass_x, rect.top+rect.height));
	double tanTopAngle = tan(GetVertAngle(target.center_mass_x, rect.top));
	height = TARGET_HEIGHT * tanBottomAngle / (tanTopAngle - tanBottomAngle);
	dist = height / tanBottomAngle; //distance along the floor
	height += TARGET_HEIGHT/2 + CAMERA_HEIGHT; //make height correspond to the middle of the target
	//printf("bA:%1f",tanBottomAngle);
}


void TargetTracker2012::GetTargets(vector<ParticleAnalysisReport>  particles){
	ParticleAnalysisReport * left, * right, * top, * bottom; 
	left = right = top = bottom = NULL;
	int targets = 0;
	double thresholdArea = particles[0].particleArea / 2;
	for (unsigned int i = 0;i < particles.size(); i++) {
		float height, dist;
		if (particles[i].particleArea < thresholdArea) {
			break;
		}
		targets++;
		GetDistAndHeight(particles[i], dist, height);
		//printf("<h: %4.1f, d1 %4.1f> ",height, dist);
		if(bottom == NULL && height < TARGET_BOT_Y + TARGET_H_MAX_ERR && height > TARGET_BOT_Y - TARGET_H_MAX_ERR) {
			//printf("bottom ");
			bottom = &particles[i];
		}
		else if(height < TARGET_MID_Y + TARGET_H_MAX_ERR && height > TARGET_MID_Y - TARGET_H_MAX_ERR) {
			//printf("middle ");
			if(left == NULL)
				left = &particles[i]; //switch these later if necissary
			else if(right == NULL)
				right = &particles[i];
		}
		else if(top == NULL && height < TARGET_TOP_Y + TARGET_H_MAX_ERR && height > TARGET_TOP_Y - TARGET_H_MAX_ERR) {
			//printf("top ");
			top = & particles[i];
		}
	}  
	
	//printf("%d targets %d particles ", targets, particles.size()); 
	
	if (right != NULL) //both mid-level targets are found
	{
		if (left->center_mass_x > right->center_mass_x) //if left is right of left, swich
		{
			ParticleAnalysisReport *temp;
			temp = left; 
			left = right;
			right = temp;
			//printf(" left and right targets switched");
		}
	}
	else if(left != NULL) { //one mid level target
		if(bottom != NULL && bottom->center_mass_x < left->center_mass_x) {
			right = left;
			left = NULL;
			//printf(" left is right");
		}
		else if(top != NULL && top->center_mass_x < left->center_mass_x)
		{
			right = left;
			left = NULL;
			//printf(" left is right");
		}
		
	}
	CreateTarget(bottom,Target::TARGET_BOTTOM);
	CreateTarget(left,Target::TARGET_LEFT);
	CreateTarget(right,Target::TARGET_RIGHT);
	CreateTarget(top,Target::TARGET_TOP);
}

void TargetTracker2012::CreateTarget(ParticleAnalysisReport *_report, Target::TargetPosition _position)
{
	Target& target = m_targets[_position];
	target.m_position = _position;
	Rect rect = _report->boundingRect;

	if(_report == NULL) {
		target.m_initialized = false;
	}
	else {
		
		target.m_report = _report;
		target.m_initialized = true;

		int cmy = _report->center_mass_y;
		int cmx = _report->center_mass_x;
		
		//calculate distance
		if(_position == Target::TARGET_TOP) 
			target.m_distance = GetDist(TARGET_TOP_Y,GetVertAngle(cmx, rect.top+rect.height/2));
		if(_position == Target::TARGET_LEFT || _position == Target::TARGET_RIGHT)
			target.m_distance =  GetDist(TARGET_MID_Y ,GetVertAngle(cmx, rect.top+rect.height/2));
		if(_position == Target::TARGET_BOTTOM)
			target.m_distance =  GetDist( TARGET_BOT_Y ,GetVertAngle(cmx, rect.top+rect.height/2));
		
		//calculate azimuth
		target.m_targetAngleWidth  = GetHorizAngle(rect.left + rect.width,cmy) - GetHorizAngle(rect.left,cmy);
		float az = GetAzimuthFromWidth(target.m_position);
		double result;
		imaqMeasureParticle(m_binImage->GetImaqImage(), _report->particleIndex, 0, IMAQ_MT_ORIENTATION, & result);
		if(result < 90)
			az = -az;
		target.m_azimuth = az;
		
		//calculate camera angle
		target.m_cameraAngle = GetHorizAngle(rect.left + rect.width/2, cmy);
	}
	target.Print();
	//printf("va: %2.1f", DEGREES_PER_RADIAN * GetVertAngle(_report->center_mass_x, rect.top+rect.height/2));
}


Position TargetTracker2012::GetRobotPosition()
{	
	vector<Position> positions;
	
	for (int a = 0; a < TARGET_COUNT; a++) // colect results
	{
		if (!m_targets[a].m_initialized)
			continue;
		
		positions.push_back(GetRobotPosition(m_targets[a].m_position));
		
		for (int b = 0; b < TARGET_COUNT; b++)
		{
			if (!m_targets[b].m_initialized)
				continue;
			if (GetTargetX(m_targets[a].m_position) == GetTargetX(m_targets[b].m_position))
				continue;
			
			positions.push_back(GetRobotPosition(m_targets[a].m_position,m_targets[b].m_position));
		}
	}
	
	m_confidence = (int)positions.size() * 5;

	Position firstAverage;
	if (positions.size() == 0)
	{

		//printf("NO TARGETS");
		return firstAverage; // set to zero 
	}
	
	printf("postions: %d",positions.size());
	vector<Position> validatedPositions;
	
	for (unsigned int a = 0; a < positions.size(); a ++ ) // initalial average
	{
		firstAverage.x += positions[a].x;
		firstAverage.z += positions[a].z;
	}
	
	firstAverage.x /= positions.size();
	firstAverage.z /= positions.size(); // set return pos to the average
	
	for (unsigned int a = 0; a < positions.size(); a ++ )
	{
		positions[a].SetError(firstAverage);
	} // set error
	
	bool change = true;
	while(change) // order the targets
	{
		change = false;
		for (unsigned int a = 0; a < positions.size() - 1; a ++ )
		{
			if (positions[a].error > positions[a+1].error) 
			{
				change = true;
				Position temp = positions[a];
				positions[a] = positions[a+1];
				positions[a+1] = temp;
			}
		} 
	}
	
	
	int newSize = (int)ceil((positions.size() + 1)/2);
	positions.resize( newSize );
	
	Position returnPos;

	
	for (unsigned int a = 0; a < positions.size(); a ++ )
	{
		returnPos.x += positions[a].x;
		returnPos.z += positions[a].z;
	}

	
	returnPos.x /= positions.size();
	returnPos.z /= positions.size(); // set return pos to the average
		

	//m_confidence -= abs(firstAverage.x - returnPos.x); 
	
	//printf("avg: " );
	//returnPos.Print();
		
	//robotPos = GetRobotPosition(Target::TARGET_LEFT, Target::TARGET_BOTTOM);
	//robotPos.Print();

	return returnPos;//positions.front();
}

Position TargetTracker2012::GetRobotPosition(Target::TargetPosition _targPos1, Target::TargetPosition _targPos2)
{
	Target& target1 = m_targets[_targPos1];
	Target& target2 = m_targets[_targPos2];
	Position position;
	if (GetTargetX(_targPos1) != GetTargetX(_targPos2))
	{
		float dist1 = target1.m_distance;
		float dist2 = target2.m_distance;
		float angleDiff = target2.m_cameraAngle - target1.m_cameraAngle;
		float xDiff = GetTargetX(_targPos2) - GetTargetX(_targPos1);
		//improve target1's azimuth
		target1.m_azimuth = acos(dist2*sin(angleDiff) / xDiff);
		if(target1.m_azimuth != target1.m_azimuth)
			target1.m_azimuth = 0; // check for infinites,negative square roots and other gargbage 
		//else if(dist2 )
		
//		float z = dist1 * dist2 * sin(angleDiff) / xDiff;
//		float x = 0;
//		if(dist1 > z)// && dist1 > dist2)
//			x = sqrt(dist1*dist1 - z*z);
//		else if(dist2 > z)
//			x = sqrt(dist2*dist2 - z*z) - xDiff;
		
		//float alpha = asin(dist1*sin(angleDiff)/xDiff) * DEGREES_PER_RADIAN;
		//float beta = asin(dist2*sin(angleDiff)/xDiff) * DEGREES_PER_RADIAN;
		float x = sin(target1.m_azimuth) * dist1;
		float z = cos(target1.m_azimuth) * dist1;		
		//printf("d1:%.1f d2: %.1f a:%.1f az: %.0f",dist1, dist2, angleDiff * DEGREES_PER_RADIAN,
				//target1.m_azimuth * DEGREES_PER_RADIAN);
		position.x = x;
		position.z = z;
		//position.x += GetTargetX(_targPos2);
	}
	else 
	{
		printf("Failure in target tracker, unitnited position line 372");
	}
	return position;
}

Position TargetTracker2012::GetRobotPosition(Target::TargetPosition _targPos)
{
	Position t_robotPosition;
	Target &t_target = m_targets[_targPos];
	
	t_robotPosition.x = sin(t_target.m_azimuth)*t_target.m_distance;
	t_robotPosition.z = cos(t_target.m_azimuth)*t_target.m_distance;
	t_robotPosition.x += GetTargetX(_targPos);
	float targetAzimuth = atan2(t_robotPosition.x, t_robotPosition.z) + M_PI;
	float camAzimuth = targetAzimuth - t_target.m_cameraAngle;
	float camCos = cos(camAzimuth);
	float camSin = sin(camAzimuth);
	
	t_robotPosition.x -= CAMERA_PARALLEL_OFFSET * camSin + CAMERA_PERPENDICULAR_OFFSET * camCos;
	t_robotPosition.z -= CAMERA_PARALLEL_OFFSET * camCos - CAMERA_PERPENDICULAR_OFFSET * camSin;
	
	return t_robotPosition;
	
}

LaunchInfo TargetTracker2012::GetLaunchVelocity(Position _robot, Target::TargetPosition _target, bool _swish){
	LaunchInfo t_launchInfo;
	float xHoop, yHoop, zHoop;
	float deltaX, deltaY, deltaZ;
	float floorDist, floorVelocity; 
	float initVertVelocity;
	float tangentEntryAngle;
	float travelTime;
	xHoop = GetTargetX(_target);
	yHoop = GetTargetY(_target)/*target height*/ - DELTA_HEIGHT;
	zHoop = _swish ? HOOP_OFFSET : -HOOP_OFFSET;
	//float angleToTarget = atan2(-_robot.x, -_robot.z);
	deltaX = xHoop - _robot.x;
	deltaY = yHoop - CAMERA_HEIGHT;
	deltaZ = zHoop - _robot.z;
	//float angleToHoop = atan2(deltaX, deltaZ);
	
	floorDist = sqrt(deltaX*deltaX+deltaZ*deltaZ);
	tangentEntryAngle = tan(DESIRED_ENTRY_ANGLE);
	floorVelocity = sqrt((GRAVITY/2)*floorDist/(tangentEntryAngle
			+ deltaY/floorDist));
	travelTime = floorDist/floorVelocity;
	initVertVelocity = GRAVITY/2 * travelTime + deltaY/travelTime;
	t_launchInfo.LaunchSpeed = sqrt(floorVelocity*floorVelocity + initVertVelocity*initVertVelocity);
	t_launchInfo.LaunchVerticalAngle = atan2(initVertVelocity,floorVelocity);
	//horizontal angle
	if(m_targets[_target].m_initialized)
		t_launchInfo.HorizAngleError = m_targets[_target].m_cameraAngle;
	else if(_target == Target::TARGET_TOP && m_targets[Target::TARGET_BOTTOM].m_initialized)
		t_launchInfo.HorizAngleError = m_targets[Target::TARGET_BOTTOM].m_cameraAngle;
	else if(_target == Target::TARGET_BOTTOM && m_targets[Target::TARGET_TOP].m_initialized)
		t_launchInfo.HorizAngleError = m_targets[Target::TARGET_TOP].m_cameraAngle;
	else
		t_launchInfo.HorizAngleError = 0;
		
	//t_launchInfo.ConfidenceLevel = m_confidence; //TODO: Implement shooting confidence!!!
	return t_launchInfo;
}

LaunchInfo TargetTracker2012::GetLaunchVelocityNoCam(Position _robot, Target::TargetPosition _target, bool _swish){ 
	//no camera input
	LaunchInfo t_launchInfo;
		float xHoop, yHoop, zHoop;
		float deltaX, deltaY, deltaZ;
		float floorDist, floorVelocity; 
		float initVertVelocity;
		float tangentEntryAngle;
		float travelTime;
		xHoop = GetTargetX(_target);
		yHoop = GetTargetY(_target)/*target height*/ - DELTA_HEIGHT;
		zHoop = _swish ? HOOP_OFFSET : -HOOP_OFFSET;
		//float angleToTarget = atan2(-_robot.x, -_robot.z);
		deltaX = xHoop - _robot.x;
		deltaY = yHoop - CAMERA_HEIGHT;
		deltaZ = zHoop - _robot.z;
		//float angleToHoop = atan2(deltaX, deltaZ);
		
		floorDist = sqrt(deltaX*deltaX+deltaZ*deltaZ);
		tangentEntryAngle = tan(DESIRED_ENTRY_ANGLE);
		floorVelocity = sqrt((GRAVITY/2)*floorDist/(tangentEntryAngle
				+ deltaY/floorDist));
		travelTime = floorDist/floorVelocity;
		initVertVelocity = GRAVITY/2 * travelTime + deltaY/travelTime;
		t_launchInfo.LaunchSpeed = sqrt(floorVelocity*floorVelocity + initVertVelocity*initVertVelocity);
		t_launchInfo.LaunchVerticalAngle = atan2(initVertVelocity,floorVelocity);
		t_launchInfo.HorizAngleError = 0; //+ (angleToHoop - angleToTarget);// - angleOffset - M_PI;//m_azimuth - angleOffset - M_PI;
		//t_launchInfo.ConfidenceLevel = m_confidence - 5; //TODO: Implement shooting confidence!!!
		return t_launchInfo;
}

float TargetTracker2012::GetTargetX(Target::TargetPosition _targPos)
{
	switch(_targPos)
	{
	case Target::TARGET_RIGHT:
		return TARGET_RHT_X;
	break;
	
	case Target::TARGET_BOTTOM:
	case Target::TARGET_TOP:
		return TARGET_MID_X;
	break;
	
	case Target::TARGET_LEFT:
		return TARGET_LFT_X;
	break;
	
	default:
		return 0.0;
	break;
	}
}

float TargetTracker2012::GetTargetY(Target::TargetPosition _targPos)
{
	switch(_targPos)
	{
	case Target::TARGET_TOP:
		return TARGET_TOP_Y;
	break;
	
	case Target::TARGET_RIGHT:
	case Target::TARGET_LEFT:
		return TARGET_MID_Y;
	break;
	
	case Target::TARGET_BOTTOM:
		return TARGET_BOT_Y;
	break;
	
	default:
		return 0.0;
	break;
	}
}


}

